%sm_dewarp
t0=clock;
xs=0.324;
ys=0.309;
n_equ=5;
x_max=960;
y_max=1280;

[dp_true,dp_warp,p_center,np]=sm_getcalidata2('needles_grid_truth.txt','needles_grid.txt',xs,ys);
p_warp=dp_warp+repmat(p_center,np,1);
A1=sm_bernstein2d3(dp_true,dp_warp,p_warp,x_max,y_max,n_equ);

p_dewarp=sm_ptdewarp(A1,p_warp,x_max,y_max,n_equ);
p_center=p_dewarp(1,:);
dp_dewarp=p_dewarp-repmat(p_center,np,1);
A2=sm_bernstein2d3(dp_warp,dp_dewarp,p_dewarp,x_max,y_max,n_equ);

p_warp2=sm_ptdewarp(A2,p_dewarp,x_max,y_max,n_equ);
a=[585,465;780,560;850,670;845,735];
a_dewarp=sm_ptdewarp(A1,a,x_max,y_max,n_equ);
a_warp=sm_ptdewarp(A2,a_dewarp,x_max,y_max,n_equ);
d=a-a_warp;
d=sqrt(d(:,1).^2+d(:,2).^2);
[mean(d),max(d),min(d)]
%a_dewarp2=sm_ptdewarp(A1,a_warp,x_max,y_max,n_equ);
%d=a_dewarp-a_dewarp2;
%d=sqrt(d(:,1).^2+d(:,2).^2);
%[mean(d),max(d),min(d)]
pc_warp=[478.841,641.26];
pc_dewarp=sm_ptdewarp(A1,pc_warp,x_max,y_max,n_equ);
pc_warp2=sm_ptdewarp(A2,pc_dewarp,x_max,y_max,n_equ);

figure;
imshow('needles1\dataset_1\images\im_00022.jpeg');
I_new1=sm_imdewarp2('needles1\dataset_1\images\im_00022.jpeg',A2,n_equ);
imwrite(I_new1,'needles1\im_00022.jpeg');
figure;
imshow('needles1\im_00022.jpeg');
figure;
imshow('needles1\dataset_1\images\im_00023.jpeg');
I_new2=sm_imdewarp2('needles1\dataset_1\images\im_00023.jpeg',A2,n_equ);
imwrite(I_new2,'needles1\im_00023.jpeg');
figure;
imshow('needles1\im_00023.jpeg');
figure;
imshow('needles1\dataset_1\images\im_00025.jpeg');
I_new3=sm_imdewarp2('needles1\dataset_1\images\im_00025.jpeg',A2,n_equ);
imwrite(I_new3,'needles1\im_00025.jpeg');
figure;
imshow('needles1\im_00025.jpeg');
figure;
imshow('needles1\dataset_1\images\im_00026.jpeg');
I_new4=sm_imdewarp2('needles1\dataset_1\images\im_00026.jpeg',A2,n_equ);
imwrite(I_new4,'needles1\im_00026.jpeg');
figure;
imshow('needles1\im_00026.jpeg');
figure;
imshow('needles1\dataset_1\images\im_00029.jpeg');
I_new5=sm_imdewarp2('needles1\dataset_1\images\im_00029.jpeg',A2,n_equ);
imwrite(I_new5,'needles1\im_00029.jpeg');
figure;
imshow('needles1\im_00029.jpeg');
figure;
imshow('needles1\dataset_1\images\im_00031.jpeg');
I_new6=sm_imdewarp2('needles1\dataset_1\images\im_00031.jpeg',A2,n_equ);
imwrite(I_new6,'needles1\im_00031.jpeg');
figure;
imshow('needles1\im_00031.jpeg');

etime(clock,t0)